#ifndef __A_STAR_HPP
#define __A_STAR_HPP


#include <string>
#include <sstream>
#include <memory>
#include <vector>
#include <unordered_map>
#include <utility>
#include <queue>
#include <stdlib.h>
#include <iostream>
#include <istream>
#include <streambuf>
#include <fstream>
#include <cmath>
#include "vec2d.h"
#include "Obstacle.h"
#include <iterator>
#include "box2d.h"
#include "path_smoother.h"

    
class PathPoint{

public:
    PathPoint() = default;
    PathPoint(double x, double y, double heading):x_(x), y_(y), heading_(heading) {}
    const double GetPhi() const{return heading_; }
    const double GetX() const{return x_; }
    const double GetY() const{return y_; }
    const double GetKappa() const{return kappa_; }
    const double GetS() const {return s_; }
    // const double GetV() const {return v_;}
    void set_s(double s) {s_ = s;}
    void set_x(double x) {x_ = x;}
    void set_y(double y) {y_ = y;}
    void set_theta(double theta) {heading_ = theta;}
    void set_kappa(double kappa) {kappa_ = kappa;}

private:
    double x_ = 0.0;
    double y_ = 0.0;
    double heading_ = 0.0;
    double s_ = 0.0;
    double kappa_ = 0.0;
};    


class Node
{
public:

    Node() = default;
    Node(const double x, const double y, const double xmin, const double ymin, 
        const double xy_resolution) {
            // x_grid_ = static_cast<int>((x - xmin) / xy_resolution);
            // y_grid_ = static_cast<int>((y - ymin) / xy_resolution);
            x_ = x;
            y_ = y;
            index_ = ComputeStringIndex(x_,y_);
        }
    Node(const int x, const int y) {
        x_ = x;
        y_ = y;
        index_ = ComputeStringIndex(x_,y_);
    }
    ~Node() = default;
    
    double GetX() {return x_;}
    double GetY() {return y_;}
    double GetCost() {return total_cost_;}
    double GetG() {return g_; }
    double GetH() {return h_; }
    double GetGridX() {return x_grid_; }
    double GetGridY() {return y_grid_; }
    double GetHeading() {return heading_; }
    std::string GetId() {return index_; }
    std::string GetIndex() {return index_; }
    std::shared_ptr<Node> GetPreNode() {return frontnode2d_; }

    void SetG(double path_cost) {g_ = path_cost; }
    void SetH(double hcost) {h_ = hcost; }
    void SetPreNode(std::shared_ptr<Node> pre) {frontnode2d_ = pre; }
    void SetCost() {total_cost_ = g_ + h_; }
    void SetIndex(std::string index) {index_ = index; }
    void SetHeading(double heading) {heading_ = heading; }

    bool operator==(const Node &rhs) {return index_ == rhs.index_; }
    bool operator<(const Node &rhs) {return total_cost_ < rhs.total_cost_; }
    bool operator>(const Node &rhs) {return total_cost_ > rhs.total_cost_; }


private:

    std::string ComputeStringIndex(int x, int y) {
        std::stringstream stream;
        stream << int(x) << "_" << int(y);
        std::string result;
        stream >> result;
        return result;
    }

    double x_,y_,heading_ = 0.0;
    int x_grid_,y_grid_ = 0;
    std::string index_ = "-1";
    double g_,h_,total_cost_ = 0.0;
    std::shared_ptr<Node> frontnode2d_ = nullptr;
};


// extern "C"{

//     std::shared_ptr<Routing> Routing_new() ;
//     void Init_Bounds(std::shared_ptr<Routing> routing) ;
// }
class Routing
{
public:
    
    Routing() = default;
    ~Routing() = default;
    void SetStartAndEndPoints(double sx,double sy,double ex,double ey);
    bool Init_Bounds();
    // bool Check_Grid(int grid_x,int grid_y);
    bool Check_Bound(double x,double y);
    double O_Cost(std::shared_ptr<Node> current_node);
    double Move_Cost(std::shared_ptr<Node> pre_node, std::shared_ptr<Node> current_node);
    // void Set_Obs_();
    bool Check_Collision(std::shared_ptr<Node> current_node); // false 无碰撞
    double NormalizeHeadingRad(double t);
    double Angle2Radian(double t);
    std::shared_ptr<Node> GenerateNextNode(std::shared_ptr<Node> current_node, int i);
    bool Search_2d();
    // std::vector<PathPoint> Init_PathPoint(std::vector<double> x, std::vector<double> y);
    void Set_Vehicleinfo(double length, double width);
    void Set_Obs();
    void Set_Resolution(double x) {xy_res_ = x;}
    bool SATcheck(Obstacle obs,Box2d vec);
    void StartOffset();
    // void EndOffset();
    bool IsTargetArea(std::shared_ptr<Node> curnode);
    bool Path_Point();
    // bool Speed_Process();
    void Write_Path();
    std::vector<double> GetTrajX() {return traj_x_;}
    std::vector<double> GetTrajY() {return traj_y_;}


private:

    void read_csv();
    struct cmp {
    bool operator()(const std::pair<std::string, double>& left,
                    const std::pair<std::string, double>& right) const {
      return left.second >= right.second;
        }
    };
    std::priority_queue<std::pair<std::string, double>,
                      std::vector<std::pair<std::string, double>>, cmp> astar_openpq_;
    std::unordered_map<std::string, std::shared_ptr<Node>> close_set_;
    std::unordered_map<std::string, std::shared_ptr<Node>> open_set_;
    std::shared_ptr<Node> start_node_;
    std::shared_ptr<Node> target_node_;
    std::shared_ptr<Node> final_node_;
    int astar_dir_ = 8;
    std::vector<int> astar_dx_ = {-1,-1,0,1,1,1,0,-1};
    std::vector<int> astar_dy_ = {0,1,1,1,0,-1,-1,-1};
    std::vector<Node> astar_result_;
    int astar_success_flag_ = 0; 

    std::vector<double> traj_x_;
    std::vector<double> traj_y_;
    std::vector<PathPoint> path_points_;
    std::vector<Obstacle> obss_;

    // 通用
    std::vector<std::vector<double>> offsetingdatas_;
    std::vector<std::vector<double>> offseteddatas_;
    std::vector<std::vector<double>> datas_;
    // std::vector<std::vector<int>> obstacle_map_;
    // std::vector<std::vector<int>> general_map_;
    double xmin_bound_ = 0.0;
    double xmax_bound_ = 0.0;
    double ymin_bound_ = 0.0;
    double ymax_bound_ = 0.0;
    // int max_x_grid_ = 0;
    // int max_y_grid_ = 0;
    double xy_res_ = 0.0;
    double offset_x_ = 0.0;
    double offset_y_ = 0.0;
    // Box2d vehicle_box_;
    double veh_length_ = 0.0;
    double veh_width_ = 0.0;
    double start_point_x_ = 0.0;
    double start_point_y_ = 0.0;
    double end_point_x_ = 0.0;
    double end_point_y_= 0.0;
};
    


#endif //__A_STAR_HPP

// extern "C" {
//     extern Routing* Routing_new() {return new Routing();}
//     extern void Init_Bounds();
// }

// std::shared_ptr<Routing> Routing_new() {
//     return std::make_shared<Routing>();
// }

// void Init_Bounds(std::shared_ptr<Routing> routing) {
//     routing->Init_Bounds();
// }
